<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang.util;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import java.awt.geom.Point2D;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import java.util.Locale;<a name="line.4"></a>
<span class="sourceLineNo">005</span><a name="line.5"></a>
<span class="sourceLineNo">006</span>import org.apache.commons.math3.complex.Quaternion;<a name="line.6"></a>
<span class="sourceLineNo">007</span>import org.apache.commons.math3.geometry.euclidean.threed.Rotation;<a name="line.7"></a>
<span class="sourceLineNo">008</span>import org.apache.commons.math3.linear.Array2DRowRealMatrix;<a name="line.8"></a>
<span class="sourceLineNo">009</span>import org.apache.commons.math3.linear.LUDecomposition;<a name="line.9"></a>
<span class="sourceLineNo">010</span>import org.apache.commons.math3.linear.MatrixUtils;<a name="line.10"></a>
<span class="sourceLineNo">011</span>import org.apache.commons.math3.linear.RealMatrix;<a name="line.11"></a>
<span class="sourceLineNo">012</span>import org.apache.commons.math3.linear.RealVector;<a name="line.12"></a>
<span class="sourceLineNo">013</span>import org.apache.commons.math3.linear.SingularValueDecomposition;<a name="line.13"></a>
<span class="sourceLineNo">014</span><a name="line.14"></a>
<span class="sourceLineNo">015</span>/**<a name="line.15"></a>
<span class="sourceLineNo">016</span> * Utility math methods used for camera calibration.<a name="line.16"></a>
<span class="sourceLineNo">017</span> * @author WB<a name="line.17"></a>
<span class="sourceLineNo">018</span> *<a name="line.18"></a>
<span class="sourceLineNo">019</span> */<a name="line.19"></a>
<span class="sourceLineNo">020</span>public class MathUtil {<a name="line.20"></a>
<span class="sourceLineNo">021</span>        <a name="line.21"></a>
<span class="sourceLineNo">022</span>        static {<a name="line.22"></a>
<span class="sourceLineNo">023</span>                Locale.setDefault(Locale.US);<a name="line.23"></a>
<span class="sourceLineNo">024</span>        }<a name="line.24"></a>
<span class="sourceLineNo">025</span>        <a name="line.25"></a>
<span class="sourceLineNo">026</span>        @Deprecated<a name="line.26"></a>
<span class="sourceLineNo">027</span>        public static void print(String name, RealMatrix M) {<a name="line.27"></a>
<span class="sourceLineNo">028</span>                System.out.println(name);<a name="line.28"></a>
<span class="sourceLineNo">029</span>                for (int r = 0; r &lt; M.getRowDimension(); r++) {<a name="line.29"></a>
<span class="sourceLineNo">030</span>                        RealVector row = M.getRowVector(r);<a name="line.30"></a>
<span class="sourceLineNo">031</span>                        System.out.println("  " + r + " " + row.toString());<a name="line.31"></a>
<span class="sourceLineNo">032</span>                }<a name="line.32"></a>
<span class="sourceLineNo">033</span>        }<a name="line.33"></a>
<span class="sourceLineNo">034</span>        <a name="line.34"></a>
<span class="sourceLineNo">035</span>        @Deprecated<a name="line.35"></a>
<span class="sourceLineNo">036</span>        public static void print(String name, RealVector v) {<a name="line.36"></a>
<span class="sourceLineNo">037</span>                System.out.println(name + v.toString());<a name="line.37"></a>
<span class="sourceLineNo">038</span>        }<a name="line.38"></a>
<span class="sourceLineNo">039</span>        <a name="line.39"></a>
<span class="sourceLineNo">040</span>        // -------------------------------------------------------<a name="line.40"></a>
<span class="sourceLineNo">041</span>        <a name="line.41"></a>
<span class="sourceLineNo">042</span>        public static double[] toArray(Point2D p) {<a name="line.42"></a>
<span class="sourceLineNo">043</span>                return new double[] {p.getX(), p.getY()};<a name="line.43"></a>
<span class="sourceLineNo">044</span>        }<a name="line.44"></a>
<span class="sourceLineNo">045</span>        <a name="line.45"></a>
<span class="sourceLineNo">046</span>        public static Point2D.Double toPoint2D(double[] xy) {<a name="line.46"></a>
<span class="sourceLineNo">047</span>                return new Point2D.Double(xy[0], xy[1]);<a name="line.47"></a>
<span class="sourceLineNo">048</span>        }<a name="line.48"></a>
<span class="sourceLineNo">049</span>        <a name="line.49"></a>
<span class="sourceLineNo">050</span>        public static RealVector toRealVector(Point2D p) {<a name="line.50"></a>
<span class="sourceLineNo">051</span>                return toRealVector(p, false);<a name="line.51"></a>
<span class="sourceLineNo">052</span>        }<a name="line.52"></a>
<span class="sourceLineNo">053</span>        <a name="line.53"></a>
<span class="sourceLineNo">054</span>        public static RealVector toRealVector(Point2D p, boolean makeHomogeneous) {<a name="line.54"></a>
<span class="sourceLineNo">055</span>                if ( makeHomogeneous) {<a name="line.55"></a>
<span class="sourceLineNo">056</span>                        return MatrixUtils.createRealVector(new double[] {p.getX(), p.getY(), 1});<a name="line.56"></a>
<span class="sourceLineNo">057</span>                }<a name="line.57"></a>
<span class="sourceLineNo">058</span>                else {<a name="line.58"></a>
<span class="sourceLineNo">059</span>                        return MatrixUtils.createRealVector(toArray(p));<a name="line.59"></a>
<span class="sourceLineNo">060</span>                }<a name="line.60"></a>
<span class="sourceLineNo">061</span>        }<a name="line.61"></a>
<span class="sourceLineNo">062</span>        <a name="line.62"></a>
<span class="sourceLineNo">063</span>        public static RealVector[] getColumnVectors(RealMatrix M) {<a name="line.63"></a>
<span class="sourceLineNo">064</span>                final int ncols = M.getColumnDimension();<a name="line.64"></a>
<span class="sourceLineNo">065</span>                RealVector[] colVecs = new  RealVector[ncols];<a name="line.65"></a>
<span class="sourceLineNo">066</span>                for (int col = 0; col &lt; ncols; col++) {<a name="line.66"></a>
<span class="sourceLineNo">067</span>                        colVecs[col] = M.getColumnVector(col);<a name="line.67"></a>
<span class="sourceLineNo">068</span>                }<a name="line.68"></a>
<span class="sourceLineNo">069</span>                return colVecs;<a name="line.69"></a>
<span class="sourceLineNo">070</span>        }<a name="line.70"></a>
<span class="sourceLineNo">071</span>        <a name="line.71"></a>
<span class="sourceLineNo">072</span>        public static RealVector crossProduct3x3(RealVector A, RealVector B) {<a name="line.72"></a>
<span class="sourceLineNo">073</span>                final double[] a = A.toArray();<a name="line.73"></a>
<span class="sourceLineNo">074</span>                final double[] b = B.toArray();<a name="line.74"></a>
<span class="sourceLineNo">075</span>                final double[] c = {<a name="line.75"></a>
<span class="sourceLineNo">076</span>                                a[1] * b[2] - b[1] * a[2],<a name="line.76"></a>
<span class="sourceLineNo">077</span>                                a[2] * b[0] - b[2] * a[0],<a name="line.77"></a>
<span class="sourceLineNo">078</span>                                a[0] * b[1] - b[0] * a[1]<a name="line.78"></a>
<span class="sourceLineNo">079</span>                };<a name="line.79"></a>
<span class="sourceLineNo">080</span>                return MatrixUtils.createRealVector(c);<a name="line.80"></a>
<span class="sourceLineNo">081</span>        }<a name="line.81"></a>
<span class="sourceLineNo">082</span>        <a name="line.82"></a>
<span class="sourceLineNo">083</span>        public static double det(RealMatrix M) {<a name="line.83"></a>
<span class="sourceLineNo">084</span>                return new LUDecomposition(M).getDeterminant();<a name="line.84"></a>
<span class="sourceLineNo">085</span>        }<a name="line.85"></a>
<span class="sourceLineNo">086</span>        <a name="line.86"></a>
<span class="sourceLineNo">087</span>        public static RealVector getRowPackedVector(RealMatrix A) {<a name="line.87"></a>
<span class="sourceLineNo">088</span>                double[][] AA = A.getData();<a name="line.88"></a>
<span class="sourceLineNo">089</span>                double[] V = new double[AA.length * AA[0].length];<a name="line.89"></a>
<span class="sourceLineNo">090</span>                int k = 0;<a name="line.90"></a>
<span class="sourceLineNo">091</span>                for (int i = 0; i &lt; AA.length; i++) {<a name="line.91"></a>
<span class="sourceLineNo">092</span>                        for (int j = 0; j &lt; AA[0].length; j++) {<a name="line.92"></a>
<span class="sourceLineNo">093</span>                                V[k++] = AA[i][j];<a name="line.93"></a>
<span class="sourceLineNo">094</span>                        }<a name="line.94"></a>
<span class="sourceLineNo">095</span>                }<a name="line.95"></a>
<span class="sourceLineNo">096</span>                return MatrixUtils.createRealVector(V);<a name="line.96"></a>
<span class="sourceLineNo">097</span>        }<a name="line.97"></a>
<span class="sourceLineNo">098</span>        <a name="line.98"></a>
<span class="sourceLineNo">099</span>        public static RealMatrix fromRowPackedVector(RealVector V, int rows, int columns) {<a name="line.99"></a>
<span class="sourceLineNo">100</span>                double[][] AA = new double[rows][columns];<a name="line.100"></a>
<span class="sourceLineNo">101</span>                double[] data = V.toArray();<a name="line.101"></a>
<span class="sourceLineNo">102</span>                int k = 0;<a name="line.102"></a>
<span class="sourceLineNo">103</span>                for (int i = 0; i &lt; rows; i++) {<a name="line.103"></a>
<span class="sourceLineNo">104</span>                        for (int j = 0; j &lt; columns; j++) {<a name="line.104"></a>
<span class="sourceLineNo">105</span>                                AA[i][j] = data[k++];<a name="line.105"></a>
<span class="sourceLineNo">106</span>                        }<a name="line.106"></a>
<span class="sourceLineNo">107</span>                }<a name="line.107"></a>
<span class="sourceLineNo">108</span>                return MatrixUtils.createRealMatrix(AA);<a name="line.108"></a>
<span class="sourceLineNo">109</span>        }<a name="line.109"></a>
<span class="sourceLineNo">110</span><a name="line.110"></a>
<span class="sourceLineNo">111</span><a name="line.111"></a>
<span class="sourceLineNo">112</span>//      /**               <a name="line.112"></a>
<span class="sourceLineNo">113</span>//       * Finds a nontrivial solution (x) to the homogeneous linear system M . x = 0.<a name="line.113"></a>
<span class="sourceLineNo">114</span>//       * @param M     <a name="line.114"></a>
<span class="sourceLineNo">115</span>//       * @param fromRight<a name="line.115"></a>
<span class="sourceLineNo">116</span>//       * @return<a name="line.116"></a>
<span class="sourceLineNo">117</span>//       */<a name="line.117"></a>
<span class="sourceLineNo">118</span>//      @Deprecated     // use the simpler version below<a name="line.118"></a>
<span class="sourceLineNo">119</span>//      public static RealVector solveHomogeneousSystemOLD(RealMatrix M, boolean fromRight) {<a name="line.119"></a>
<span class="sourceLineNo">120</span>//              SingularValueDecomposition svd = new SingularValueDecomposition(M);<a name="line.120"></a>
<span class="sourceLineNo">121</span>//              RealMatrix U = svd.getU();<a name="line.121"></a>
<span class="sourceLineNo">122</span>//              RealMatrix V = svd.getV();<a name="line.122"></a>
<span class="sourceLineNo">123</span>//              <a name="line.123"></a>
<span class="sourceLineNo">124</span>//              // dimensions of the original (decomposed) matrix;<a name="line.124"></a>
<span class="sourceLineNo">125</span>//              int svdnumRows = U.getRowDimension();<a name="line.125"></a>
<span class="sourceLineNo">126</span>//              int svdnumCols = V.getColumnDimension();<a name="line.126"></a>
<span class="sourceLineNo">127</span>//              double[] s = svd.getSingularValues();<a name="line.127"></a>
<span class="sourceLineNo">128</span>//              <a name="line.128"></a>
<span class="sourceLineNo">129</span>//              RealMatrix A = fromRight ? svd.getVT() /*V is transposed!*/ : svd.getU();<a name="line.129"></a>
<span class="sourceLineNo">130</span>//                    <a name="line.130"></a>
<span class="sourceLineNo">131</span>//              // find the row/column index of the smallest singular value (diagonal)<a name="line.131"></a>
<span class="sourceLineNo">132</span>//              int minIndex = -1;<a name="line.132"></a>
<span class="sourceLineNo">133</span>//              if (fromRight &amp;&amp; svdnumCols &gt; svdnumRows)<a name="line.133"></a>
<span class="sourceLineNo">134</span>//                      minIndex = svdnumCols - 1;<a name="line.134"></a>
<span class="sourceLineNo">135</span>//              else if (!fromRight &amp;&amp; svdnumCols &lt; svdnumRows)<a name="line.135"></a>
<span class="sourceLineNo">136</span>//                      minIndex = svdnumRows - 1;<a name="line.136"></a>
<span class="sourceLineNo">137</span>//              else {<a name="line.137"></a>
<span class="sourceLineNo">138</span>//                      // find the index of the smallest singular value<a name="line.138"></a>
<span class="sourceLineNo">139</span>//                      double minValue = Double.MAX_VALUE;<a name="line.139"></a>
<span class="sourceLineNo">140</span>//                      for (int i = 0; i &lt; s.length; i++) {<a name="line.140"></a>
<span class="sourceLineNo">141</span>//                              if (s[i] &lt; minValue) {<a name="line.141"></a>
<span class="sourceLineNo">142</span>//                                      minValue = s[i];<a name="line.142"></a>
<span class="sourceLineNo">143</span>//                                      minIndex = i;<a name="line.143"></a>
<span class="sourceLineNo">144</span>//                              }<a name="line.144"></a>
<span class="sourceLineNo">145</span>//                      }<a name="line.145"></a>
<span class="sourceLineNo">146</span>//              }<a name="line.146"></a>
<span class="sourceLineNo">147</span>//              //System.out.println("nullspace: smallestIndex = " + minIndex);<a name="line.147"></a>
<span class="sourceLineNo">148</span>//              <a name="line.148"></a>
<span class="sourceLineNo">149</span>//              RealVector nullVec = fromRight ? A.getRowVector(minIndex) : A.getColumnVector(minIndex);<a name="line.149"></a>
<span class="sourceLineNo">150</span>//              return nullVec;<a name="line.150"></a>
<span class="sourceLineNo">151</span>//      }<a name="line.151"></a>
<span class="sourceLineNo">152</span>        <a name="line.152"></a>
<span class="sourceLineNo">153</span>        <a name="line.153"></a>
<span class="sourceLineNo">154</span>        /**               <a name="line.154"></a>
<span class="sourceLineNo">155</span>         * Finds a nontrivial solution (x) to the homogeneous linear system M . x = 0<a name="line.155"></a>
<span class="sourceLineNo">156</span>         * by singular-value decomposition. If M has more rows than columns, the<a name="line.156"></a>
<span class="sourceLineNo">157</span>         * system of equations is overdetermined. In this case the returned solution<a name="line.157"></a>
<span class="sourceLineNo">158</span>         * minimizes the residual ||M . x|| in the least-squares sense.<a name="line.158"></a>
<span class="sourceLineNo">159</span>         * <a name="line.159"></a>
<span class="sourceLineNo">160</span>         * @param A     the original matrix.<a name="line.160"></a>
<span class="sourceLineNo">161</span>         * @return the solution vector x.<a name="line.161"></a>
<span class="sourceLineNo">162</span>         */<a name="line.162"></a>
<span class="sourceLineNo">163</span>        public static RealVector solveHomogeneousSystem(RealMatrix A) {<a name="line.163"></a>
<span class="sourceLineNo">164</span>                SingularValueDecomposition svd = new SingularValueDecomposition(A);<a name="line.164"></a>
<span class="sourceLineNo">165</span>                RealMatrix V = svd.getV();<a name="line.165"></a>
<span class="sourceLineNo">166</span>                RealVector x = V.getColumnVector(V.getColumnDimension() - 1);<a name="line.166"></a>
<span class="sourceLineNo">167</span>                return x;<a name="line.167"></a>
<span class="sourceLineNo">168</span>        }<a name="line.168"></a>
<span class="sourceLineNo">169</span>        <a name="line.169"></a>
<span class="sourceLineNo">170</span>        public static double[] toHomogeneous(double[] cvec) {<a name="line.170"></a>
<span class="sourceLineNo">171</span>                double[] hvec = new double[cvec.length + 1];<a name="line.171"></a>
<span class="sourceLineNo">172</span>                for (int i = 0; i &lt; cvec.length; i++) {<a name="line.172"></a>
<span class="sourceLineNo">173</span>                        hvec[i] = cvec[i];<a name="line.173"></a>
<span class="sourceLineNo">174</span>                        hvec[hvec.length - 1] = 1;<a name="line.174"></a>
<span class="sourceLineNo">175</span>                }<a name="line.175"></a>
<span class="sourceLineNo">176</span>                return hvec;<a name="line.176"></a>
<span class="sourceLineNo">177</span>        }<a name="line.177"></a>
<span class="sourceLineNo">178</span>        <a name="line.178"></a>
<span class="sourceLineNo">179</span>        public static double[] toCartesian(double[] hvec) {<a name="line.179"></a>
<span class="sourceLineNo">180</span>                double[] cvec = new double[hvec.length - 1];<a name="line.180"></a>
<span class="sourceLineNo">181</span>                final double s = 1 / hvec[hvec.length - 1];     // TODO: check for zero factor<a name="line.181"></a>
<span class="sourceLineNo">182</span>                for (int i = 0; i &lt; hvec.length - 1; i++) {<a name="line.182"></a>
<span class="sourceLineNo">183</span>                        cvec[i] = s * hvec[i];<a name="line.183"></a>
<span class="sourceLineNo">184</span>                }<a name="line.184"></a>
<span class="sourceLineNo">185</span>                return cvec;<a name="line.185"></a>
<span class="sourceLineNo">186</span>        }<a name="line.186"></a>
<span class="sourceLineNo">187</span>        <a name="line.187"></a>
<span class="sourceLineNo">188</span>//      /**  <a name="line.188"></a>
<span class="sourceLineNo">189</span>//       * implements a left (pre-) matrix-vector multiplication:  A . x -&gt; y<a name="line.189"></a>
<span class="sourceLineNo">190</span>//       * Matrix A is of size (m,n), column vector x is of length n.<a name="line.190"></a>
<span class="sourceLineNo">191</span>//       * The result y is a vector of length m.<a name="line.191"></a>
<span class="sourceLineNo">192</span>//       * non-destructive<a name="line.192"></a>
<span class="sourceLineNo">193</span>//       */<a name="line.193"></a>
<span class="sourceLineNo">194</span>//      public static double[] multiply(final double[][] A, final double[] x) {<a name="line.194"></a>
<span class="sourceLineNo">195</span>//              double[] y = new double[A.length];<a name="line.195"></a>
<span class="sourceLineNo">196</span>//              multiplyD(A, x, y);<a name="line.196"></a>
<span class="sourceLineNo">197</span>//              return y;<a name="line.197"></a>
<span class="sourceLineNo">198</span>//      }<a name="line.198"></a>
<span class="sourceLineNo">199</span>        <a name="line.199"></a>
<span class="sourceLineNo">200</span>//      // destructive<a name="line.200"></a>
<span class="sourceLineNo">201</span>//      public static void multiplyD(final double[][] A, final double[] x, double[] y) {<a name="line.201"></a>
<span class="sourceLineNo">202</span>//              final int m = A.length;<a name="line.202"></a>
<span class="sourceLineNo">203</span>//              final int n = A[0].length;<a name="line.203"></a>
<span class="sourceLineNo">204</span>//              if (x.length != n || y.length != m) <a name="line.204"></a>
<span class="sourceLineNo">205</span>//                      throw new IllegalArgumentException("incompatible matrix-vector dimensions");<a name="line.205"></a>
<span class="sourceLineNo">206</span>//              for (int i = 0; i &lt; m; i++) {<a name="line.206"></a>
<span class="sourceLineNo">207</span>//                      double s = 0;<a name="line.207"></a>
<span class="sourceLineNo">208</span>//                      for (int j = 0; j &lt; n; j++) {<a name="line.208"></a>
<span class="sourceLineNo">209</span>//                              s = s + A[i][j] * x[j];<a name="line.209"></a>
<span class="sourceLineNo">210</span>//                      }<a name="line.210"></a>
<span class="sourceLineNo">211</span>//                      y[i] = s;<a name="line.211"></a>
<span class="sourceLineNo">212</span>//              }<a name="line.212"></a>
<span class="sourceLineNo">213</span>//      }<a name="line.213"></a>
<span class="sourceLineNo">214</span>        <a name="line.214"></a>
<span class="sourceLineNo">215</span>        public static String getInfo(double[][] A) {<a name="line.215"></a>
<span class="sourceLineNo">216</span>                return String.format("Matrix: rows=%d, columns=%d", A.length, A[0].length);<a name="line.216"></a>
<span class="sourceLineNo">217</span>        }<a name="line.217"></a>
<span class="sourceLineNo">218</span>        <a name="line.218"></a>
<span class="sourceLineNo">219</span>        public static String getInfo(double[] x) {<a name="line.219"></a>
<span class="sourceLineNo">220</span>                return String.format("Vector: dimension=%d", x.length);<a name="line.220"></a>
<span class="sourceLineNo">221</span>        }<a name="line.221"></a>
<span class="sourceLineNo">222</span>        <a name="line.222"></a>
<span class="sourceLineNo">223</span>        public static double mean(double[] x) {<a name="line.223"></a>
<span class="sourceLineNo">224</span>                final int n = x.length;<a name="line.224"></a>
<span class="sourceLineNo">225</span>                if (n == 0) <a name="line.225"></a>
<span class="sourceLineNo">226</span>                        return 0;<a name="line.226"></a>
<span class="sourceLineNo">227</span>                double sum = 0;<a name="line.227"></a>
<span class="sourceLineNo">228</span>                for (int i = 0; i &lt; x.length; i++) {<a name="line.228"></a>
<span class="sourceLineNo">229</span>                        sum = sum + x[i];<a name="line.229"></a>
<span class="sourceLineNo">230</span>                }<a name="line.230"></a>
<span class="sourceLineNo">231</span>                return sum / n;<a name="line.231"></a>
<span class="sourceLineNo">232</span>        }<a name="line.232"></a>
<span class="sourceLineNo">233</span>        <a name="line.233"></a>
<span class="sourceLineNo">234</span>        /**<a name="line.234"></a>
<span class="sourceLineNo">235</span>         * <a name="line.235"></a>
<span class="sourceLineNo">236</span>         * @param x a sequence of real values <a name="line.236"></a>
<span class="sourceLineNo">237</span>         * @return the variance of the values in x (sigma^2)<a name="line.237"></a>
<span class="sourceLineNo">238</span>         */<a name="line.238"></a>
<span class="sourceLineNo">239</span>        public static double variance(double[] x) {<a name="line.239"></a>
<span class="sourceLineNo">240</span>                final int n = x.length;<a name="line.240"></a>
<span class="sourceLineNo">241</span>                if (n == 0) <a name="line.241"></a>
<span class="sourceLineNo">242</span>                        return 0;<a name="line.242"></a>
<span class="sourceLineNo">243</span>                double sum = 0;<a name="line.243"></a>
<span class="sourceLineNo">244</span>                double sum2 = 0;<a name="line.244"></a>
<span class="sourceLineNo">245</span>                for (int i = 0; i &lt; x.length; i++) {<a name="line.245"></a>
<span class="sourceLineNo">246</span>                        sum = sum + x[i];<a name="line.246"></a>
<span class="sourceLineNo">247</span>                        sum2 = sum2 + x[i] * x[i];<a name="line.247"></a>
<span class="sourceLineNo">248</span>                }<a name="line.248"></a>
<span class="sourceLineNo">249</span>                return (sum2 - (sum * sum) / n) / n;<a name="line.249"></a>
<span class="sourceLineNo">250</span>        }<a name="line.250"></a>
<span class="sourceLineNo">251</span>        <a name="line.251"></a>
<span class="sourceLineNo">252</span>        <a name="line.252"></a>
<span class="sourceLineNo">253</span>        // Conversions between Double[] and double[]<a name="line.253"></a>
<span class="sourceLineNo">254</span>        <a name="line.254"></a>
<span class="sourceLineNo">255</span>//      // We want to pass a 'Deque' instance, thus not just 'List' parameter!<a name="line.255"></a>
<span class="sourceLineNo">256</span>//      public static double[] toArray(Collection&lt;Double&gt; c) {<a name="line.256"></a>
<span class="sourceLineNo">257</span>//              double[] model = new double[c.size()];<a name="line.257"></a>
<span class="sourceLineNo">258</span>//              int i = 0;<a name="line.258"></a>
<span class="sourceLineNo">259</span>//              for (Double x : c) {<a name="line.259"></a>
<span class="sourceLineNo">260</span>//                      System.out.format("Param %d = %.6f\n", i, x);<a name="line.260"></a>
<span class="sourceLineNo">261</span>//                      if (x != null) {<a name="line.261"></a>
<span class="sourceLineNo">262</span>//                              model[i] = x.doubleValue();<a name="line.262"></a>
<span class="sourceLineNo">263</span>//                              i++;<a name="line.263"></a>
<span class="sourceLineNo">264</span>//                      }<a name="line.264"></a>
<span class="sourceLineNo">265</span>//              }<a name="line.265"></a>
<span class="sourceLineNo">266</span>//              return model;<a name="line.266"></a>
<span class="sourceLineNo">267</span>//      }<a name="line.267"></a>
<span class="sourceLineNo">268</span>        <a name="line.268"></a>
<span class="sourceLineNo">269</span>//      public static Deque&lt;Double&gt; toList(double[] a) {<a name="line.269"></a>
<span class="sourceLineNo">270</span>//              Deque&lt;Double&gt; A = new ArrayDeque&lt;Double&gt;(a.length);<a name="line.270"></a>
<span class="sourceLineNo">271</span>//              for (int i = 0; i &lt; a.length; i++) {<a name="line.271"></a>
<span class="sourceLineNo">272</span>//                      A.add(a[i]);<a name="line.272"></a>
<span class="sourceLineNo">273</span>//              }<a name="line.273"></a>
<span class="sourceLineNo">274</span>//              return A;<a name="line.274"></a>
<span class="sourceLineNo">275</span>//      }<a name="line.275"></a>
<span class="sourceLineNo">276</span>        <a name="line.276"></a>
<span class="sourceLineNo">277</span>        // ---------------------------------------------------------------<a name="line.277"></a>
<span class="sourceLineNo">278</span>        <a name="line.278"></a>
<span class="sourceLineNo">279</span>        public static Quaternion toQuaternion(Rotation r) {<a name="line.279"></a>
<span class="sourceLineNo">280</span>                return new Quaternion(r.getQ0(), r.getQ1(), r.getQ2(), r.getQ3());<a name="line.280"></a>
<span class="sourceLineNo">281</span>        }<a name="line.281"></a>
<span class="sourceLineNo">282</span>        <a name="line.282"></a>
<span class="sourceLineNo">283</span>        public static Rotation toRotation(Quaternion q) {<a name="line.283"></a>
<span class="sourceLineNo">284</span>                return new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true);<a name="line.284"></a>
<span class="sourceLineNo">285</span>        }<a name="line.285"></a>
<span class="sourceLineNo">286</span>        <a name="line.286"></a>
<span class="sourceLineNo">287</span>        // ---------------------------------------------------------------<a name="line.287"></a>
<span class="sourceLineNo">288</span>        <a name="line.288"></a>
<span class="sourceLineNo">289</span>        /**<a name="line.289"></a>
<span class="sourceLineNo">290</span>         * Calculates the 'inverse condition number' (RCOND() in Matlab)<a name="line.290"></a>
<span class="sourceLineNo">291</span>         * of the given matrix (0,...,1, ideally close to 1).<a name="line.291"></a>
<span class="sourceLineNo">292</span>         * @param Ma the matrix<a name="line.292"></a>
<span class="sourceLineNo">293</span>         * @return the inverse condition number<a name="line.293"></a>
<span class="sourceLineNo">294</span>         */<a name="line.294"></a>
<span class="sourceLineNo">295</span>        public static double inverseConditionNumber(double[][] Ma) {<a name="line.295"></a>
<span class="sourceLineNo">296</span>                RealMatrix M = new Array2DRowRealMatrix(Ma);<a name="line.296"></a>
<span class="sourceLineNo">297</span>                SingularValueDecomposition svd = new SingularValueDecomposition(M);<a name="line.297"></a>
<span class="sourceLineNo">298</span>                return svd.getInverseConditionNumber();<a name="line.298"></a>
<span class="sourceLineNo">299</span>        }<a name="line.299"></a>
<span class="sourceLineNo">300</span>        <a name="line.300"></a>
<span class="sourceLineNo">301</span>        <a name="line.301"></a>
<span class="sourceLineNo">302</span>        // ---------------------------------------------------------------<a name="line.302"></a>
<span class="sourceLineNo">303</span>        <a name="line.303"></a>
<span class="sourceLineNo">304</span>        /**<a name="line.304"></a>
<span class="sourceLineNo">305</span>         * For testing only.<a name="line.305"></a>
<span class="sourceLineNo">306</span>         * @param args ignored<a name="line.306"></a>
<span class="sourceLineNo">307</span>         */<a name="line.307"></a>
<span class="sourceLineNo">308</span>        public static void main (String[] args) {<a name="line.308"></a>
<span class="sourceLineNo">309</span>                //double[][] A = {{1, 2, 3}, {4, 5, 6}, {9, 8, 0}};<a name="line.309"></a>
<span class="sourceLineNo">310</span>                double[][] A = {{1, 2, 3}, {4, 5, 6}, {9, 8, 0}, {-3, 7, 2}};<a name="line.310"></a>
<span class="sourceLineNo">311</span>                {<a name="line.311"></a>
<span class="sourceLineNo">312</span>                        RealMatrix AM = MatrixUtils.createRealMatrix(A);<a name="line.312"></a>
<span class="sourceLineNo">313</span>                        RealVector x = solveHomogeneousSystem(AM);<a name="line.313"></a>
<span class="sourceLineNo">314</span>                        System.out.println("Solution x = " + x.toString());<a name="line.314"></a>
<span class="sourceLineNo">315</span>                }<a name="line.315"></a>
<span class="sourceLineNo">316</span>                // Solution x = {0.649964237; -0.7338780288; 0.1974070146}<a name="line.316"></a>
<span class="sourceLineNo">317</span>        }<a name="line.317"></a>
<span class="sourceLineNo">318</span>        <a name="line.318"></a>
<span class="sourceLineNo">319</span><a name="line.319"></a>
<span class="sourceLineNo">320</span>}<a name="line.320"></a>




























































</pre>
</div>
</body>
</html>
